#include "Postion.h"

#include "imgprodcons.h"
Solve_Posetion::Solve_Posetion() {
    _Cam_Aberration_Martix = (Mat_<double>(5, 1) << -0.06,0.26,0,0,-0.4);//畸变矩阵赋值参数
    _Cam_Instant_Matrix = (Mat_<double>(3, 3) << 1.12e3, 0, 6.53e2, 0,1.12e3, 5.4e2, 0, 0, 1);//内参矩阵赋值参数
}
double Solve_Posetion::getDistance(){
   // cout << _TVec.at<double>(0, 0)<<"   "<<_TVec.at<double>(2, 0)<<"   "<<_TVec.at<double>(1, 0)<<endl;

    return Distance;
}

const Vec2f Solve_Posetion::getAngle(int Classer){
    if (Classer == 1) {
        //因为步兵要的是绝对坐标系
        _XErr = _RVec.at<double>(1, 0) - atan(_TVec.at<double>(0, 0) / _TVec.at<double>(2, 0)) / 2 / CV_PI * 360;
        _YErr = _RVec.at<double>(2, 0) - atan(_TVec.at<double>(1, 0) / _TVec.at<double>(2, 0)) / 2 / CV_PI * 360;
    }
    if(Classer==0){
        _XErr = atan(_TVec.at<double>(0, 0) / _TVec.at<double>(2, 0)) / 2 / CV_PI * 360;
	    _YErr = atan(_TVec.at<double>(1, 0) / _TVec.at<double>(2, 0)) / 2 / CV_PI * 360;
    }
    return cv::Vec2f(_XErr, _YErr);
}
void Solve_Posetion::SolvePnP(){
    //预处理轮廓结果
    solvePnP(POINT_3, POIN_2, _Cam_Instant_Matrix, _Cam_Aberration_Martix, _RVec, _TVec);
   // _XErr = atan(_tVec.at<double>(0, 0) / _tVec.at<double>(2, 0)) / 2 / CV_PI * 360;
	//_YErr = atan(_tVec.at<double>(1, 0) / _tVec.at<double>(2, 0)) / 2 / CV_PI * 360;
    Distance =sqrt(_TVec.at<double>(0, 0)*_TVec.at<double>(0, 0) + _TVec.at<double>(1, 0)*_TVec.at<double>(1, 0) + _TVec.at<double>(2, 0)* _TVec.at<double>(2, 0));
    //PnP函数
}
void Solve_Posetion::Debug() {
    cout << _Cam_Aberration_Martix << endl;
    cout << "-------------------" << endl;
    cout << _Cam_Instant_Matrix << endl;;
    

    SolvePnP();//PnP解算
    cout << getDistance() << endl;//测距
    cout << _TVec << endl;
    cout << _RVec << endl;
  //  cout << _TVec << endl;
}
void Solve_Posetion::Set_3D_Point(int targetClass) {
    /*
    * 本代码为测试代码
    */
    POINT_3.clear();
    if (targetClass == 1) {
        POINT_3.push_back(Point3f(-0.07, 0.03, 0));//步兵
        POINT_3.push_back(Point3f(-0.07, -0.03, 0));
        POINT_3.push_back(Point3f(0.07, 0.03, 0));
        POINT_3.push_back(Point3f(0.07, -0.03, 0));
    }
    //with  =  230
    //hight =120
    else if (targetClass == 2) {
        POINT_3.push_back(Point3f(-0.115, 0.03, 0));// 英雄
        POINT_3.push_back(Point3f(-0.115, -0.03, 0));
        POINT_3.push_back(Point3f(0.115, 0.03, 0));
        POINT_3.push_back(Point3f(0.115, -0.03, 0));
    }
    else if (targetClass == 3) {
        POINT_3.push_back(Point3f(0, 0, 0));//拓展
        POINT_3.push_back(Point3f(137, 0, 0));
        POINT_3.push_back(Point3f(137, 98, 0));
        POINT_3.push_back(Point3f(0, 98, 0));
    
    }
}
void Solve_Posetion::Set_2D_Point(Point2f *tar) {
    //Point2f Ptr[5];
   // Rect.points(Ptr);
    POIN_2.clear();
    POIN_2.push_back(tar[0]);
    POIN_2.push_back(tar[1]);
    POIN_2.push_back(tar[2]);
    POIN_2.push_back(tar[3]);
}
